#ifndef OCCUPANCY_MAP_H
#define OCCUPANCY_MAP_H

#include <cv_bridge/CvBridge.h>
#include <ros/ros.h>
#include <ext/hash_map>
#include <map>
#include <string>
#include <vector>
#include <list>

#include "common.h"

#define OCCUPIED 255
#define FREE 0
#define UNKNOWN 127
#define FRONTIER -1

#define BLACK 0
#define GREY 127
#define WHITE 255

typedef std::map<Coordinates, int> Map;
typedef std::vector<const Trajectory* > Trajectories;

class OccupancyMap {
 public:
	OccupancyMap();
	int getXY(int x, int y) const;
	int getGridXY(int x, int y) const;
	void setXY(int x, int y, int value);
	void clearXY(int x, int y);
	int getSize(void) const;
	sensor_msgs::Image::Ptr getImgMessage(void);
	void merge(const OccupancyMap& new_map);
	const Map& getMapHandle(void) const;
	void addTrajectory(const Trajectory *trajectory);
	void addTrajectoryList(const Trajectories *traj_list);
	int getMinX(void) const;
	int getMaxX(void) const;
	int getMinY(void) const;
	int getMaxY(void) const;
	void euclideToGrid(Coordinates *pose) const;
	void gridToEuclide(Coordinates *pose) const;

 private:
	void drawPose_(const Coordinates &pose, IplImage *img, const CvScalar &color);
	void setPixel_(IplImage *img, int x, int y, const CvScalar &color);
	const CvScalar getColor_(int idx);
	Map map_;
	int min_x_;
	int min_y_;
	int max_x_;
	int max_y_;
	Trajectories trajectories_;
	const Trajectories *trajectory_list_;
	std::vector<CvScalar> colors_;
};

#endif

